﻿using Microsoft.Kinect;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Kinect.Toolbox.Comparison;

namespace TigerController.Comparison
{
    private struct Degree
    {
        public double X { get; set; }

        public double Y { get; set; }

        public double Z { get; set; }
    }

    public static class ComparisonSystem
    {
        public static ScoreOfSpecificJoint[] CalculateScore(
            Skeleton sampleSkeleton,
            Skeleton currentSkeleton,
            ComparisonRule ruleOfComparison)
        {
            ScoreOfSpecificJoint[] scoreOfSpecificJoints = new ScoreOfSpecificJoint[20];
            foreach (JointType jointType in ruleOfComparison.ComparisonTypes)
            {
                scoreOfSpecificJoints[(Int32)jointType] = CalculateScoreForSpecificJoint(
                    jointType,
                    sampleSkeleton.BoneOrientations[jointType],
                    currentSkeleton.BoneOrientations[jointType],
                    ruleOfComparison);
            }
            return scoreOfSpecificJoints;
        }

        private static ScoreOfSpecificJoint CalculateScoreForSpecificJoint(
            JointType jointType,
            BoneOrientation sampleBoneOrientation,
            BoneOrientation currentBoneOrientation,
            ComparisonRule ruleOfComparison)
        {
            ScoreOfSpecificJoint scoreOfSpecificJoint = new ScoreOfSpecificJoint();
            scoreOfSpecificJoint.SpecificJointType = jointType;

            //Single degreeOfSampleBoneOrientationSingleRepresentative = sampleBoneOrientation.HierarchicalRotation.Matrix.M11;
            //Single degreeOfCurrentBoneOrientationSingleRepresentative = currentBoneOrientation.HierarchicalRotation.Matrix.M11;
            //Double degreeOfDifferenceDoubleRepresentative = Math.Abs(
            //    degreeOfSampleBoneOrientationSingleRepresentative
            //    - degreeOfCurrentBoneOrientationSingleRepresentative);
            //Double degreeOfDifference = Math.Acos(degreeOfDifferenceDoubleRepresentative);
            //scoreOfSpecificJoint.Score = GetScore(degreeOfDifference, ruleOfComparison);
            Degree sampleDegree = GetDegree(sampleBoneOrientation);
            Degree currentDegree = GetDegree(currentBoneOrientation);
            int result = (GetScore(currentDegree.X - sampleDegree.X, ruleOfComparison) + GetScore(currentDegree.Y - sampleDegree.Y, ruleOfComparison) + GetScore(currentDegree.Z - sampleDegree.Z, ruleOfComparison)) / 3;
            scoreOfSpecificJoint.Score = result;
            return scoreOfSpecificJoint;
        }

        private static Int32 GetScore(double degreeOfDifference, ComparisonRule ruleOfComparison)
        {
            Double score = 0d;
            degreeOfDifference = Math.Abs(degreeOfDifference);
            if (degreeOfDifference < ruleOfComparison.MinimumRadian)
            {
                score = 100d;
            }
            else if (degreeOfDifference > ruleOfComparison.MaximumRadian)
            {
                score = 0d;
            }
            else
            {
                score = (1 - degreeOfDifference / (ruleOfComparison.MaximumRadian - ruleOfComparison.MinimumRadian)) * 100;
            }
            return (Int32)score;
        }

        private static Degree GetDegree(BoneOrientation orientation)
        {
            Matrix4 matrix = orientation.HierarchicalRotation.Matrix;
            Vector4 quaternion = orientation.HierarchicalRotation.Quaternion;
            var returnValue = new Degree();
            returnValue.Y = quaternion.Y;
            returnValue.X = quaternion.X;
            returnValue.Z = quaternion.Z;
            return returnValue;
        }
    }
}
